#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include "pid.h"


/******************************************************************************
 *函数名称：  setPidParameter
 *输入参数：  pid,pid控制体指针
 *            pidK、pidI、pidD,比例、积分、微分参数
 * 			  max、min：输出限幅，可正可负，为0表示不限幅
 *输出参数：  无
 *返回值  ：  无
 *功能描述：  设置PID参数，并初始化
 ******************************************************************************/
void setPidParameter(PID_CTL *pid, float pidK, float pidI, float pidD, float max, float min)
{
	pid->pid_Kp = pidK;
	pid->pid_Ki = pidI;
	pid->pid_Kd = pidD;
	
	pid->max_out = max;
	pid->min_out = min;	
	
	pid->preErr = 0;	//初始化误差值为0


	pid->prePreErr = 0;	//初始化上上次误差为0
	pid->pid_preU = 0;	//初始化pid值

	pid->AccuErr = 0;	//积累误差初始化为0

}

/******************************************************************************
 *函数名称：  setPidTarget
 *输入参数：  pid,pid控制体指针
 *            target,控制输出的目标值
 *输出参数：  无
 *返回值  ：  无
 *功能描述：  设置PID目标值
 ******************************************************************************/
void setPidTarget(PID_CTL *pid, float target)
{
	pid->pid_target = target;
}

/******************************************************************************
 *函数名称：  getPIDCtrlValue
 *输入参数：  pid,pid控制体指针
 *            feedback,反馈的实际值
 *输出参数：  无
 *返回值  ：  叠加PID后的控制值
 *功能描述：  传入反馈值，PID计算后返回最终的控制值
 ******************************************************************************/
float getPIDCtrlValue(PID_CTL *pid, float feedback, bool posi)
{
	float pid_err; 				//计算输出的误差
	float deltaU;				//计算控制信号的增量值
	
	//计算误差
	pid_err = pid->pid_target - feedback;

	if(posi == true) {
		//累加积分误差
		pid->AccuErr += pid_err;
		//PID计算
		//u[t]=Kp*e[t]+Ki(e[0]+e[1]+e[2]+...+e[t−2]+e[t−1]+e[t])+Kd(e[t]−e[t−1])
		deltaU = pid->pid_Kp * pid_err 						//比例	
					+ pid->pid_Ki * pid->AccuErr			//积分
					+ pid->pid_Kd * (pid_err - pid->preErr);//微分

		pid->preErr = pid_err;		//把当前误差赋值给preErr，供下次使用
	} else {
		//PI计算
		//u[t]=Kp(e[t]−e[t−1])+Ki×e[t]+Kd(e[t]−2e[t−1]+e[t−2])+u[t−1]
		deltaU = pid->pid_Kp * (pid_err - pid->preErr) 		//比例	
					+ pid->pid_Ki * pid_err					//积分
					+ pid->pid_Kd * (pid_err - 2 * pid->preErr + pid->prePreErr) //微分
					+ pid->pid_preU;						//上一次的pid值
				
		pid->prePreErr = pid->preErr;	//把上一次的误差赋给上上次误差值，供下次使用
		pid->preErr = pid_err;			//把当前误差赋值给preErr，供下次使用
		
		pid->pid_preU = deltaU;			//把当前pid值保存起来供下次使用
	}


	pid->pid_out = deltaU;							//控制信号的值

	if(pid->max_out != 0.0f) {						//限幅有效性判断
		if(pid->pid_out > pid->max_out) {			//设置限幅操作
			pid->pid_out = pid->max_out;
		}
	}
	
	if(pid->min_out != 0.0f) {
		if(pid->pid_out < pid->min_out) {
			pid->pid_out = pid->min_out;
		}
	}
	return pid->pid_out;
}
